/*
 * FEMCalcBase3D.cpp
 *
 *      Author: Jerome RD Soine and Christoph A Brand
 *      Institution: Schwarz goup, Institute for Theoretical Physics, Heidelberg University, Germany
 */

#include "../../../../include/base/substrate_models/FEM/FEMCalcBase.h"

FEMCalcBase::FEMCalcBase()
{
    ref_parameter = 0.4;
    triangulation = new Triangulation<3>();
    dof_handler = new DoFHandler<3> (*triangulation);
    dof_handler_force = new DoFHandler<3> (*triangulation);
    fe = new FESystem<3> (FE_Q<3>(1), 3);
    fe_force = new FESystem<3> (FE_Q<3>(1), 3);
    n_forces = 0;
    first_calculation = true;
    first_force_projection = true;
}

FEMCalcBase::~FEMCalcBase()
{
	delete dof_handler;
	delete dof_handler_force;
	delete triangulation;
	delete fe;
	delete fe_force;
}

/*!
 * Generate 3D rectangular mesh and initialize force DoFs.
 */
void FEMCalcBase::generate_rectangular_mesh(Point<3> p1, Point<3> p2, vector<ForceTransmission*> force_transmisions, unsigned int n_refs)
{
	// Initialize global variables
	force_trans=force_transmisions;

	triangulation->set_mesh_smoothing(dealii::Triangulation<3,3>::smoothing_on_refinement);
	GridGenerator::hyper_rectangle(*triangulation,p1,p2,true);


	/*
	 * Use local refinement scheme.
	 */
	if(n_refs>=3)
	{
		triangulation->refine_global(3);
		n_refs=n_refs-3;

		if(n_refs<=1)
		{
			for(unsigned int i=0;i<n_refs;i++)
						refine_surface();
		}
		else
		{
			for(unsigned int i=0;i<1;i++)
						refine_surface();
			n_refs = n_refs-1;
			for(unsigned int i=0;i<n_refs;i++)
				refine_based_on_traction_shapes(force_transmisions);
		}
	}
	else
	{
		triangulation->refine_global(n_refs);
	}
	dof_handler->distribute_dofs(*fe);
	dof_handler_force->distribute_dofs(*fe_force);
	solution.reinit(dof_handler->n_dofs());
	force_solution.reinit(dof_handler_force->n_dofs());
	reinit_force();
}

void FEMCalcBase::mesh_import(std::string filename)
{
    std::cout << "Import Mesh File " << filename << " ... \n";
    GridIn<3> grid_in;
    grid_in.attach_triangulation(*triangulation);
    std::ifstream input_file(filename.c_str());
    grid_in.read_msh(input_file);
    dof_handler->distribute_dofs(*fe);
    dof_handler_force->distribute_dofs(*fe_force);
    solution.reinit(dof_handler->n_dofs());
    force_solution.reinit(dof_handler_force->n_dofs());

    reinit_force();
}

void FEMCalcBase::mesh_import_with_history(std::string mesh_file, std::string history_file)
{
    std::cout << "Import Mesh File "<< mesh_file << " with history " << history_file << " ... \n";
    GridIn<3> grid_in;
    grid_in.attach_triangulation(*triangulation);
    std::ifstream input_file(mesh_file.c_str());
    grid_in.read_msh(input_file);

    std::ifstream history (history_file.c_str());
    triangulation->load_refine_flags(history);
    triangulation->execute_coarsening_and_refinement();

    dof_handler->distribute_dofs(*fe);
    dof_handler_force->distribute_dofs(*fe_force);
    solution.reinit(dof_handler->n_dofs());
    force_solution.reinit(dof_handler_force->n_dofs());

    reinit_force();
}


void FEMCalcBase::mesh_export(std::string filename)
{
    std::cout << "Export Mesh " << filename << "... \n";
    std::ofstream data_out (filename.c_str());
    GridOutFlags::Msh msh_flags (true,false);
    GridOut grid_out;
    grid_out.set_flags(msh_flags);
    grid_out.write_msh(*triangulation,data_out);
    data_out.close();
}

void FEMCalcBase::output(std::string filename)
{
  std::cout << "Writing solution vtk-file " << filename << "... \n";
  std::ofstream output (filename.c_str());
  DataOut<3> data_out;
  data_out.attach_dof_handler (*dof_handler);

  std::vector<std::string> solution_names;
  solution_names.push_back ("x_displacement");
  solution_names.push_back ("y_displacement");
  solution_names.push_back ("z_displacement");

  data_out.add_data_vector (solution, solution_names);
  data_out.build_patches ();
  data_out.write_vtk (output);
}

void FEMCalcBase::output_force(std::string filename)
{
  std::cout << "Writing force vtk-file " << filename << "... \n";
  std::ofstream output (filename.c_str());
  DataOut<3> data_out;
  data_out.attach_dof_handler (*dof_handler_force);

  std::vector<std::string> solution_names;
  solution_names.push_back ("x_force");
  solution_names.push_back ("y_force");
  solution_names.push_back ("z_force");

  data_out.add_data_vector (force_solution, solution_names);
  data_out.build_patches ();
  data_out.write_vtk (output);

}

unsigned int FEMCalcBase::n_dofs()
{
  return dof_handler->n_dofs();
}

unsigned int FEMCalcBase::n_force_dofs()
{
  return n_forces;
}

unsigned int FEMCalcBase::n_vertices()
{
  return(triangulation->n_vertices());
}

void FEMCalcBase::refine_globaly()
{
  triangulation->refine_global (1);
  dof_handler->distribute_dofs(*fe);
  dof_handler_force->distribute_dofs(*fe_force);
  solution.reinit(dof_handler->n_dofs());
  force_solution.reinit(dof_handler_force->n_dofs());
  reinit_force();

}

void FEMCalcBase::refine_surface()
{
  Triangulation<3>::active_cell_iterator cell = triangulation->begin_active(),
					 endc = triangulation->end();

  for (; cell!=endc; ++cell)
  {
    for (unsigned int face=0; face<GeometryInfo<3>::faces_per_cell; ++face)
    {
      if (cell->face(face)->at_boundary() && cell->face(face)->boundary_indicator() == boundary_index) cell->set_refine_flag();
    }
  }
  triangulation->execute_coarsening_and_refinement();

  dof_handler->distribute_dofs(*fe);
  dof_handler_force->distribute_dofs(*fe_force);
  solution.reinit(dof_handler->n_dofs());
  force_solution.reinit(dof_handler_force->n_dofs());
  reinit_force();
}

void FEMCalcBase::refine_adaptively()
{
  std::cout << "FEMCalc refine adaptive ... \n" << std::endl;

  std::ofstream history ("mesh.history");

  // Set Refinement Flag by Error Estimation
  Vector<float> estimated_error_per_cell (triangulation->n_active_cells());

  /*typename*/ FunctionMap<3>::type neumann_boundary;
  KellyErrorEstimator<3>::estimate (*dof_handler,
                                     QGauss<2>(3),
                                     neumann_boundary,
                                     solution,
                                     estimated_error_per_cell);

   GridRefinement::refine_and_coarsen_fixed_number (*triangulation,
                                                    estimated_error_per_cell,
                                                    ref_parameter, 0.0);

   // Refinement only at the Boundary Surface -> Unset Refinement Flags if cell is not at the boundary
   DoFHandler<3>::active_cell_iterator cell = dof_handler->begin_active(),
				       endc = dof_handler->end();
   bool clear_refine_flag;	// Is true if cell refine flag shall be cleared
   for (; cell!=endc; ++cell)
   {
     clear_refine_flag=true;
     for (unsigned int face=0; face<GeometryInfo<3>::faces_per_cell; ++face)
      {
	  if (cell->face(face)->at_boundary() && (cell->face(face)->boundary_indicator() == boundary_index)) clear_refine_flag=false;
	  else;
      }
     if(clear_refine_flag==true) cell->clear_refine_flag();
   }
   triangulation->save_refine_flags(history);
   triangulation->execute_coarsening_and_refinement();

  dof_handler->distribute_dofs(*fe);
  dof_handler_force->distribute_dofs(*fe_force);
  solution.reinit(dof_handler->n_dofs());
  force_solution.reinit(dof_handler_force->n_dofs());
  reinit_force();
}


void FEMCalcBase::reinit_force()
{

    std::vector<bool> dof_is_force (dof_handler_force->n_dofs());
    for(unsigned int i=0;i<dof_is_force.size();i++) dof_is_force[i] = false;

    unsigned int vertices_per_face = GeometryInfo<3>::vertices_per_face;
    DoFHandler<3>::active_cell_iterator cell = dof_handler_force->begin_active(),
					endc = dof_handler_force->end();

    for (; cell!=endc; ++cell)
    {
      for (unsigned int face=0; face<GeometryInfo<3>::faces_per_cell; ++face)
      {
	 if (cell->face(face)->at_boundary() && cell->face(face)->boundary_indicator() == boundary_index)
	 {
	    for(unsigned int i=0;i<vertices_per_face;i++)
	    {
	      for(unsigned int j=0;j<3;j++)
	      {
		  dof_is_force[cell->face(face)->vertex_dof_index(i,j)]=true;
	      }
	    }
	 }
      }
    }

    force_indices.resize(0);
    for(unsigned i=0;i<dof_is_force.size();i++)
      if(dof_is_force[i]) force_indices.push_back(i);


    n_forces = force_indices.size();

    force.reinit(n_forces);
    for(unsigned int i=0;i<force.size();i++)
    	force(i) = force_solution(force_indices[i]);
}

void FEMCalcBase::reinitialize_values(Vector< double > f)
{


    assert(f.size()==n_forces);		// Look if sizes match
    force_solution.reinit(dof_handler_force->n_dofs());					// Set solution to zero
    for(unsigned int i=0;i<f.size();i++)
      force_solution(force_indices[i]) = f(i);
    force.reinit(n_forces);
    force = f;
}


void FEMCalcBase::save_solution_vector(std::string filename)
{
    std::ofstream data (filename.c_str());
    for(unsigned int i=0;i<solution.size();i++)
      data << solution(i) << "\n";
    data.close();
}

void FEMCalcBase::save_force_vector(std::string filename)
{
    std::ofstream data (filename.c_str());
    for(unsigned int i=0;i<force_solution.size();i++)
      data << force_solution(i) << "\n";
    data.close();
}

double FEMCalcBase::force_sum()
{
    // Calculate the square of the force sum.
    double f_sum=0;
    for(unsigned int i=0;i<force_solution.size();i++)
        f_sum+=force_solution(i)*force_solution(i);
    return(f_sum);
}

void FEMCalcBase::move_mesh()
{
       /*
       * Move Mesh according to the computated solution
       */
      std::cout << "Moving mesh..." << std::endl;

      std::vector<bool> vertex_touched (triangulation->n_vertices(), false);

      for (DoFHandler<3>::active_cell_iterator cell = dof_handler->begin_active (); cell != dof_handler->end(); ++cell)
       for (unsigned int v=0; v<GeometryInfo<3>::vertices_per_cell; ++v)
	    if (vertex_touched[cell->vertex_index(v)] == false)
	    {
		vertex_touched[cell->vertex_index(v)] = true;

		Point<3> vertex_displacement;
		for (unsigned int d=0; d<3; ++d)
		      vertex_displacement[d] = solution(cell->vertex_dof_index(v,d));

		cell->vertex(v) += vertex_displacement;
	    }
}


void FEMCalcBase::point_evaluation(std::vector< Point< 3 > >& points, std::vector< dealii::Vector< double > >& value_list)
{
     /*Check dimensions matching of value vectors and point vectors */
    Assert (value_list.size() == points.size(),ExcDimensionMismatch (value_list.size(), points.size()));

    Functions::FEFieldFunction<3> fe_function (*dof_handler, solution);
    Timer timer;
    timer.start();
    fe_function.vector_value_list(points,value_list);
    std::cout << "Point interpolations took " << timer() << "s" << std::endl;

}

void FEMCalcBase::point_evaluation(std::vector< Point< 3 > >& points, std::vector< dealii::Vector< double > >& value_list, std::vector<DoFHandler<3>::active_cell_iterator>& cell_iterators)
{
    assert (points.size() == cell_iterators.size());
    assert (points.size() == value_list.size());

    Functions::FEFieldFunction<3> fe_function (*dof_handler, solution);
    for(unsigned int i=0;i<points.size();i++)
    {
      fe_function.set_active_cell(cell_iterators[i]);
      fe_function.vector_value(points[i],value_list[i]);
    }
}

void FEMCalcBase::point_evaluation(Point<3> &p,Vector<double> &v)
{
	Functions::FEFieldFunction<3> fe_function(*dof_handler, solution);
	fe_function.vector_value(p,v);
}

vector<double> FEMCalcBase::get_displacement_at(vector<double> point)
{
	Point<3> p = Point<3>(point[0],point[1],0);
	Vector<double> v (2);
	Functions::FEFieldFunction<3> fe_function(*dof_handler, solution);
	fe_function.vector_value(p,v);
	vector<double> tmp (2,0);
	tmp[0] = v[0];
	tmp[1] = v[1];
	return tmp;

}

vector<vector<double> > FEMCalcBase::get_displacement_list_at(vector<vector<double> > points)
{
	unsigned int n_points = points.size();
	vector<Point<3> > pts (n_points);
	vector<Vector<double> > vecs (n_points);
	for(unsigned int i=0;i<n_points;i++){
		vecs[i].reinit(3);
	}
	for(unsigned int i=0;i<n_points;i++){
		pts[i] = Point<3> (points[i][0],points[i][1],0);
	}

	if(first_calculation)
	{
		for(unsigned int i=0;i<n_points;i++){
			cell_iterator_list.push_back(find_cell_around_point(pts[i]));
		}
		first_calculation = false;
	}


	Functions::FEFieldFunction<3> fe_function(*dof_handler, solution);
	for(unsigned int i=0;i<n_points;i++)
	{
		fe_function.set_active_cell(cell_iterator_list[i]);
		fe_function.vector_value(pts[i],vecs[i]);
	}

	vector<vector<double> > out_vecs;
	vector<double> tmp (2);
	for(unsigned int i=0;i<n_points;i++)
	{
		tmp [0] = vecs[i][0];
		tmp [1] = vecs[i][1];
		out_vecs.push_back(tmp);
	}
	return out_vecs;
}


Vector< double > FEMCalcBase::get_force_vector()
{
  return(force);
}

void FEMCalcBase::set_refine_parameter(double r_para)
{
  ref_parameter = r_para;
}

Vector<double> FEMCalcBase::get_force_solution_vector()
{
	return force_solution;
}

Vector< double > FEMCalcBase::get_solution()
{
  return (solution);
}

void FEMCalcBase::set_solution(dealii::Vector< double > sol)
{
  assert(solution.size()==sol.size());
  solution = sol;
}

void FEMCalcBase::set_force_solution(Vector<double> f)
{
	assert(f.size()==force_solution.size());
	force_solution = f;
}


DoFHandler< 3  >::active_cell_iterator FEMCalcBase::find_cell_around_point(Point< 3 > p)
{
  return GridTools::find_active_cell_around_point<3>(*dof_handler,p);
}


Triangulation<3> FEMCalcBase::get_triangulation()
{
	Triangulation<3> tria;
	tria.copy_triangulation(*triangulation);
	return tria;
}

double FEMCalcBase::get_total_traction()
{

	//Vector<double> Traction (3);
	double Traction =0;

	DoFHandler<3>::active_cell_iterator cell = dof_handler_force->begin_active(),
						endc = dof_handler_force->end();
	QGauss<2>  face_quadrature_formula(2);

	FEFaceValues<3> fe_force_values (*fe_force,face_quadrature_formula,
	                           update_values | update_q_points | update_JxW_values);

	FEFaceValues<3> fe_face_values (*fe, face_quadrature_formula,
	                           update_values | update_q_points  | update_JxW_values);


	const unsigned int   n_face_q_points = face_quadrature_formula.size();
	const unsigned int   dofs_per_face = fe->dofs_per_face;

	  std::vector<unsigned int>	local_dof_indices (dofs_per_face);
	  std::vector<Vector<double> > 	rhs_values (n_face_q_points, Vector<double>(3));

	  std::vector<Tensor<1,3> > sol_vec (n_face_q_points);




	  const FEValuesExtractors::Vector displacements (0);


	  for (; cell!=endc; ++cell)
	  {
	      for (unsigned int face=0; face<GeometryInfo<3>::faces_per_cell; ++face)
	      {
	    	  if (cell->face(face)->at_boundary() && (cell->face(face)->boundary_indicator() == boundary_index))
	    	  {
	    		  cell->face(face)->get_dof_indices (local_dof_indices);
	    		  fe_force_values.reinit(cell,face);
	    		  fe_face_values.reinit(cell,face);
	    		  fe_force_values[displacements].get_function_values(force_solution,sol_vec);
	    		  for(unsigned int i=0;i<dofs_per_face;i++)
	    		  {

	    			  unsigned int j = fe->face_to_cell_index(i,face);
	    			  const unsigned int component_i = fe->system_to_component_index(j).first;
	    			  if(component_i==0)
	    			  {
	    			  for (unsigned int q_point=0; q_point<n_face_q_points; ++q_point)
	    				  	  	  Traction += std::sqrt(sol_vec[q_point][0]*sol_vec[q_point][0]+sol_vec[q_point][1]*sol_vec[q_point][1])*fe_face_values.shape_value(j,q_point)*fe_force_values.JxW(q_point);
	                           //Traction += std::sqrt(std::pow(sol_vec[q_point][0],2.0)+std::pow(sol_vec[q_point][1],2.0));//*fe_force_values.JxW(q_point);
	    			  }
	    		  }
	    	  }
	      }
	  }

	  return Traction;


	 //Alternative implementation
    /*
	unsigned int n_dofs = dof_handler_force->n_dofs();
	vector<bool> is_used (n_dofs,false);
	double total_traction = 0;
	Vector<double> f(2);
	DoFHandler<3>::active_cell_iterator cell = dof_handler_force->begin_active(),
											   endc = dof_handler_force->end();

	double area;

	for (; cell!=endc; ++cell)
	{
		      for (unsigned int face=0; face<GeometryInfo<3>::faces_per_cell; ++face)
		      {
		    	  if (cell->face(face)->at_boundary() && (cell->face(face)->boundary_indicator() == boundary_index))
		    	  {
		    		  for (unsigned int v=0; v<GeometryInfo<3>::vertices_per_face; ++v)
		    		  {
		    			  if(!is_used[cell->face(face)->vertex_dof_index(v,0)])
		    			  {
		    		 					   f(0) = force_solution(cell->face(face)->vertex_dof_index(v,0));
		    		 					   f(1) = force_solution(cell->face(face)->vertex_dof_index(v,1));
		    		 					   total_traction += f.l2_norm()*cell_area;
		    		 					   is_used[cell->face(face)->vertex_dof_index(v,0)] = true;
		    			  }
		    		 }
		    	  }
		      }
	}
	return total_traction;
	*/
}

void FEMCalcBase::project_traction(vector<ForceTransmission*> force_transmission)
{
	force_trans = force_transmission;

	unsigned int n_dofs = dof_handler_force->n_dofs();

	Vector<double> f (n_dofs);

	force_solution.reinit(n_dofs);

	DoFHandler<3>::active_cell_iterator cell = dof_handler_force->begin_active(),
									     endc = dof_handler_force->end();
    triangulation->clear_user_data();

    if(first_force_projection)
    {
    	for (; cell!=endc; ++cell)
    	{
    	    for (unsigned int face=0; face<GeometryInfo<3>::faces_per_cell; ++face)
    	  	{
    	    	if (cell->face(face)->boundary_indicator() == boundary_index)
    	    	{
    	    		bool has_vertex_force = false;
    	    		vector<unsigned int> tmp_force_indices;
    	    		for (unsigned int v=0; v<GeometryInfo<3>::vertices_per_face; ++v)
    	    		{
    	    			for(unsigned int i=0; i<force_trans.size();i++)
    	    			{
    	    				vector<double> vertex_point (2,0);
    	    				vertex_point[0] = cell->face(face)->vertex(v)[0];
    	    				vertex_point[1] = cell->face(face)->vertex(v)[1];
    	    				if(force_trans[i]->get_shape()->is_on_shape(vertex_point))
    	    				{
    	    					has_vertex_force = true;
    	    					tmp_force_indices.push_back(i);
    	    				}
    	    			}
    	    		}
    	    		if(has_vertex_force)
    	    		{
    	    			traction_faces.push_back(cell->face(face));
    	    			face_to_force_indices.push_back(tmp_force_indices);
    	    		}
    	    	}

    	  	}
    	}
    	first_force_projection=false;
    }


    for(unsigned int i=0;i<traction_faces.size();i++)
    {
    	for (unsigned int v=0; v<GeometryInfo<3>::vertices_per_face; ++v)
    	{
    		vector<unsigned int> tmp_force_indices = face_to_force_indices[i];
    		for(unsigned int j=0; j<tmp_force_indices.size();j++)
    		{
    			vector<double> vertex_point (2,0);
    		    vertex_point[0] = traction_faces[i]->vertex(v)[0];
    		    vertex_point[1] = traction_faces[i]->vertex(v)[1];
    		    if(force_trans[tmp_force_indices[j]]->get_shape()->is_on_shape(vertex_point))
    		    {
    		    		double area = force_trans[tmp_force_indices[j]]->get_shape()->get_area();
    		    		f(traction_faces[i]->vertex_dof_index(v,0))=force_trans[tmp_force_indices[j]]->get_force()[0]/area;
    		    		f(traction_faces[i]->vertex_dof_index(v,1))=force_trans[tmp_force_indices[j]]->get_force()[1]/area;
    		    }
    		}
    	}
    }

	force_solution = f;

}

vector<double> point_to_std_vector(dealii::Point<3> &point)
{
	vector<double> tmp (2);
	tmp[0] = point[0];
	tmp[1] = point[1];
	return tmp;
}

void FEMCalcBase::refine_based_on_traction_shapes(vector<ForceTransmission*> force_transmission)
{

	force_trans=force_transmission;

	DoFHandler<3>::active_cell_iterator cell = dof_handler_force->begin_active(),
												endc = dof_handler_force->end();

	for (; cell!=endc; ++cell)
	{
	    	for (unsigned int face=0; face<GeometryInfo<3>::faces_per_cell; ++face)
	  		{
	    		if (cell->face(face)->boundary_indicator() == boundary_index)
	    		{
	    			for(unsigned int i=0; i<force_trans.size();i++)
	    			{
	    				vector<double> tmp = force_trans[i]->get_shape()->get_center();
	    				Point<3> p (tmp[0],tmp[1],0);
	    				if(cell->point_inside(p))
	    					cell->set_refine_flag();
	    			}

	    			for (unsigned int v=0; v<GeometryInfo<3>::vertices_per_face; ++v)
	    			{
	    				for(unsigned int i=0; i<force_trans.size();i++)
	    				{
	    					vector<double> vertex_point (2,0);
	    					vertex_point[0] = cell->face(face)->vertex(v)[0];
	    					vertex_point[1] = cell->face(face)->vertex(v)[1];
	    					if(force_trans[i]->get_shape()->is_on_shape(vertex_point))
	    					{
	    						cell->set_refine_flag();
	    					}
	    				}
	     			}
	    		}
	  		}
	}

	triangulation->execute_coarsening_and_refinement();

}

